package com.rockwell.sniffyhunter.model;

import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;

import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;

import com.rockwell.sniffyhunter.utils.DetectionIndex;
import com.rockwell.sniffyhunter.utils.IDetector;
import com.rockwell.sniffyhunter.views.OnColorBlobDetectListener;

public class ColorBlobDetector {
    // Lower and Upper bounds for range checking in HSV color space
    private Scalar mLowerBound = new Scalar(0);
    private Scalar mUpperBound = new Scalar(0);
    // Minimum contour area in percent for contours filtering
    private static double mMinContourArea = 0.5;
    // Color radius for range checking in HSV color space
    //private Scalar mColorRadius = new Scalar(25,50,50,0);
    private Scalar mColorRadius = new Scalar(12,25,25,0);
    private Size SPECTRUM_SIZE = new Size(200, 64);
    private Scalar CONTOUR_COLOR = new Scalar(255,0,0,255);
    private Scalar NAME_COLOR = new Scalar(0,0,255,255);
    private Mat mSpectrum = new Mat();
    private List<MatOfPoint> mContours = new ArrayList<MatOfPoint>();
    
    private Scalar m_contourColor = new Scalar(0);
    
    private OnColorBlobDetectListener m_detectionListener;
    public int m_detectionIndex = 0;
    private String m_name;

    // Cache
    Mat mPyrDownMat = new Mat();
    Mat mHsvMat = new Mat();
    Mat mMask = new Mat();
    Mat mDilatedMask = new Mat();
    Mat mHierarchy = new Mat();

    public ColorBlobDetector(Scalar contourColor, int index, String name) {
    	m_contourColor = contourColor;
    	m_detectionIndex = index;
    	m_name = name;
    }
    
    public void setColorRadius(Scalar radius) {
        mColorRadius = radius;
    }
    
    public void setDetectorListener(OnColorBlobDetectListener l) {
    	m_detectionListener = l;
	}
    
    public int getDetectionIndex() {
    	return m_detectionIndex;
    }

    public void setHsvColor(Scalar hsvColor) {
        double minH = (hsvColor.val[0] >= mColorRadius.val[0]) ? hsvColor.val[0]-mColorRadius.val[0] : 0;
        double maxH = (hsvColor.val[0]+mColorRadius.val[0] <= 255) ? hsvColor.val[0]+mColorRadius.val[0] : 255;

        mLowerBound.val[0] = minH;
        mUpperBound.val[0] = maxH;

        mLowerBound.val[1] = hsvColor.val[1] - mColorRadius.val[1];
        mUpperBound.val[1] = hsvColor.val[1] + mColorRadius.val[1];

        mLowerBound.val[2] = hsvColor.val[2] - mColorRadius.val[2];
        mUpperBound.val[2] = hsvColor.val[2] + mColorRadius.val[2];

        mLowerBound.val[3] = 0;
        mUpperBound.val[3] = 255;

        Mat spectrumHsv = new Mat(1, (int)(maxH-minH), CvType.CV_8UC3);

        for (int j = 0; j < maxH-minH; j++) {
            byte[] tmp = {(byte)(minH+j), (byte)255, (byte)255};
            spectrumHsv.put(0, j, tmp);
        }

        Imgproc.cvtColor(spectrumHsv, mSpectrum, Imgproc.COLOR_HSV2RGB_FULL, 4);
    }

    public Mat getSpectrum() {
        return mSpectrum;
    }

    public void setMinContourArea(double area) {
        mMinContourArea = area;
    }
    
    public void detect(Mat rgbaImage) {
    	process(rgbaImage);
    	
        List<MatOfPoint> contours = getContours();
        Imgproc.drawContours(rgbaImage, contours, -1, CONTOUR_COLOR);
        
        ColorBlobInfo info = new ColorBlobInfo();
        boolean present = drawContourCenters(rgbaImage, info);
        
        if (m_detectionListener != null)
        	m_detectionListener.onDetect(m_detectionIndex, present, info.center, info.radius, rgbaImage.height(), rgbaImage.width());
    }

    public void process(Mat rgbaImage) {
        Imgproc.pyrDown(rgbaImage, mPyrDownMat);
        Imgproc.pyrDown(mPyrDownMat, mPyrDownMat);

        Imgproc.cvtColor(mPyrDownMat, mHsvMat, Imgproc.COLOR_RGB2HSV_FULL);

        Core.inRange(mHsvMat, mLowerBound, mUpperBound, mMask);
        Imgproc.dilate(mMask, mDilatedMask, new Mat());

        List<MatOfPoint> contours = new ArrayList<MatOfPoint>();

        Imgproc.findContours(mDilatedMask, contours, mHierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE);

        // Find max contour area
        double maxArea = 0;
        Iterator<MatOfPoint> each = contours.iterator();
        while (each.hasNext()) {
            MatOfPoint wrapper = each.next();
            double area = Imgproc.contourArea(wrapper);
            if (area > maxArea)
                maxArea = area;
        }

        // Filter contours by area and resize to fit the original image size
        mContours.clear();
        each = contours.iterator();
        while (each.hasNext()) {
            MatOfPoint contour = each.next();
            if (Imgproc.contourArea(contour) > mMinContourArea*maxArea) {
                Core.multiply(contour, new Scalar(4,4), contour);
                mContours.add(contour);
            }
        }
    }
    
    public boolean drawContourCenters(Mat rgbaImage, ColorBlobInfo info) {
    	float[] r = {0.0f};
    	Point c = new Point();
    	
    	float[] curR = {0.0f};
    	Point curC = new Point();
    	
        Iterator<MatOfPoint> each = mContours.iterator();
        while (each.hasNext()) {
        	
        	MatOfPoint contour = each.next();
        	
            MatOfPoint2f contourf = new MatOfPoint2f(contour.toArray());

            Imgproc.minEnclosingCircle(contourf, curC, curR);
            
            if (curR[0] > r[0]) {
            	r[0] = curR[0];
            	c.x = curC.x;
            	c.y = curC.y;
            }
        }

        if ((c.x != 0 || c.y != 0) && r[0] >= IDetector.MIN_RADIUS[m_detectionIndex]) {
        	info.center.x = c.x;
        	info.center.y = c.y;
        	info.radius = r[0];
        	
        	Core.circle(rgbaImage, c, 3, new Scalar(0,0,255), 2, 8, 0);
            Core.circle(rgbaImage, c, (int)r[0], m_contourColor, 2, 8, 0);
        	
            int width = rgbaImage.width();
            int height = rgbaImage.height();
            
            if (m_detectionIndex == DetectionIndex.MAZE_EXIT) {
            	
            	Point corner1 = new Point(width * IDetector.FOV_LOWER_WIDTH_BOUNDARY_MAZE_EXIT, height * IDetector.FOV_LOWER_HEIGHT_BOUNDARY_MAZE_EXIT);
            	Point corner2 = new Point(width * IDetector.FOV_UPPER_WIDTH_BOUNDARY_MAZE_EXIT, height * IDetector.FOV_UPPER_HEIGHT_BOUNDARY_MAZE_EXIT);
            	
            	Core.rectangle(rgbaImage, corner1, corner2, m_contourColor, 2);
        	}
            else if (m_detectionIndex == DetectionIndex.MAZE1 ||
            		 m_detectionIndex == DetectionIndex.MAZE2 ||
            		 m_detectionIndex == DetectionIndex.MAZE3) {
            	
            	Point corner1 = new Point(width * IDetector.FOV_LOWER_WIDTH_BOUNDARY_MAZE, height * IDetector.FOV_LOWER_HEIGHT_BOUNDARY_MAZE);
            	Point corner2 = new Point(width * IDetector.FOV_UPPER_WIDTH_BOUNDARY_MAZE, height * IDetector.FOV_UPPER_HEIGHT_BOUNDARY_MAZE);
            	
            	Core.rectangle(rgbaImage, corner1, corner2, m_contourColor, 2);
        	}
            else {
	        	Core.line(rgbaImage, new Point(width * IDetector.FOV_WIDTH_BOUNDARY, height * IDetector.FOV_HEIGHT_BOUNDARY), 
	        						 new Point(width * IDetector.FOV_WIDTH_BOUNDARY, height * (1 - IDetector.FOV_HEIGHT_BOUNDARY)), 
	        						 m_contourColor,
	        						 2);
	        	
	        	Core.line(rgbaImage, new Point(0, height * IDetector.FOV_HEIGHT_BOUNDARY), 
						 new Point(width * IDetector.FOV_WIDTH_BOUNDARY, height * IDetector.FOV_HEIGHT_BOUNDARY), 
						 m_contourColor,
						 2);
	        	
	        	Core.line(rgbaImage, new Point(0, rgbaImage.height() * (1 - IDetector.FOV_HEIGHT_BOUNDARY)), 
						 new Point(width * IDetector.FOV_WIDTH_BOUNDARY, height * (1 - IDetector.FOV_HEIGHT_BOUNDARY)), 
						 m_contourColor,
						 2);
            }
        	
        	double centerX = 0;
        	double centerY = 0;
        	double halfTolerance = 0;
        	
        	if (m_detectionIndex == DetectionIndex.TREASURE) {
        		centerX = width * IDetector.TREASURE_CENTER_POSITION_X;
            	centerY = height * IDetector.TREASURE_CENTER_POSITION_Y;
            	halfTolerance = (height * IDetector.TREASURE_CENTER_TOLERANCE)/2.0f;
        	}
        	else {
        		centerX = width * IDetector.OBSTACLE_CENTER_POSITION_X;
            	centerY = height * IDetector.OBSTACLE_CENTER_POSITION_Y;
            	halfTolerance = (height * IDetector.OBSTACLE_CENTER_TOLERANCE)/2.0f;
        	}
        	
            
        	Point corner1 = new Point(centerX - halfTolerance, centerY - halfTolerance);
        	Point corner2 = new Point(centerX + halfTolerance, centerY + halfTolerance);
        	
        	Core.rectangle(rgbaImage, corner1, corner2, m_contourColor, 2);
        	
        	String displayInfo = String.format("%s(%.0f,%.0f)", m_name, info.center.x, info.center.y);
        	Core.putText(rgbaImage, displayInfo, info.center, Core.FONT_HERSHEY_SIMPLEX, 1.5, NAME_COLOR);
        	return true;
        }
        else
        	return false;
    }

    public List<MatOfPoint> getContours() {
        return mContours;
    }
}
